//	  conduction_velocity.cxx
//
//	  Copyright 2011 Stefan Fruhner <stefan.fruhner@gmail.com>
//
//	  This program is free software; you can redistribute it and/or modify
//	  it under the terms of the GNU General Public License as published by
//	  the Free Software Foundation; either version 2 of the License, or
//	  (at your option) any later version.
//
//	  This program is distributed in the hope that it will be useful,
//	  but WITHOUT ANY WARRANTY; without even the implied warranty of
//	  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//	  GNU General Public License for more details.
//
//	  You should have received a copy of the GNU General Public License
//	  along with this program; if not, write to the Free Software
//	  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
//	  MA 02110-1301, USA.


#include <iostream>
#include <mylibs/myinifiles.hpp>
#include <mylibs/CardiacMesh.hpp>
#include <mylibs/cmdline.hpp>
#include <myImg/IGBheader.hpp>
#include <assert.h>
#include <gsl/gsl_fit.h>
using namespace mylibs;

using namespace mylibs;

int main(int argc, char **argv){
	myIniFiles ini(argc, argv);

	ini.set_info("'conduction_velocity' computes the conduction velocity (cv)\
 of the activation front. It needs two files: (1) the mesh in CARP format and \
 (2) and a file which contains the activation times for each node in this mesh.\
 cv is computed by linear regression over all neighboring points of a node.\
 The equation c T = X is solved where T are the differences in activation time\
 and X the difference in x-direction for example. Both are known for every\
 neighboring node. That velocity that fits best to the data is determined.");

	ini.register_param("input", "i", "Basename of the mesh (CARP format). ATTENTION: INPUT data is assumed to be im um !");
	ini.register_param("output", "o", "[ = filename] Name of the output to be written, e.g. cv.dat or cv.igb.gz. The output data is in cm/s.");
	ini.register_param("activation", "a", "Activation file, which contains activation time in ms");
	ini.register_param("method", "m", "['leastsquares', 'stupid'] Activation file, which contains activation time in ms", "leastsquares");
	ini.register_param("depth", "d", "<int> : Number of iterations how far away the neighbors are to be taken into account (some kind of coarse graining).");
	ini.register_flag("verbose", "v", "Give additional messages");
	ini.register_flag("mean", "c", "Compute the mean value using all neighbors of a point");

	ini.check();
	mystring output 		 = ini.read("output", mystring("cv.igb.gz"));
	mystring activation_file = ini.read("activation", mystring("init_acts_vm_act-thresh.dat"));
	mystring mesh 			 = ini.read("input", 	mystring());
	mystring method 		 = ini.read("method", mystring("leastsquares"));

	size_t depth 			 = ini.read("depth", 0);

	bool compute_mean		 = ini.exists("mean");
	bool verbose 			 = ini.exists("verbose");

	mesh.file_new_ext("pts");

//////////////////// Read the files ////////////////////////////////////////////
	CardiacMesh m(mesh);
	vector<double> act(m.points());

	if (verbose) cmdline::msg(" - Reading activation file");
	ifstream infile(activation_file.c_str());
	assert(!!infile &&  "Error opening activtion file");

	for (size_t ii=0; ii<m.points(); ii++){
		infile >> act[ii];
		if (not infile.good())
			cmdline::exit("Line: " +toString(ii)
				+ " : Infile not good ! Possibly the sizes of the mesh"
				+ " and the activation file do not match.");
	}

	if (not infile.eof()) {
		assert(!!infile && "Did not reach the end of the file as expected, so there is data left in the file.");
	}
	infile.close();

//////////////////////////////// Compute cv ////////////////////////////////////

	if (verbose) cmdline::msg(" - Initialization");
	vector<Point> cv(m.points(), Point()); 		// conduction velocities
	vector<Point> stddev(m.points(), Point()); 	// conduction velocity stddev

	if (verbose) cmdline::msg(" - Computing cv values");
//	#pragma omp parallel for
	for (size_t i = 0; i < m.points(); ++i){
		if (act[i] <= 0.) continue; // skip points that were not acitvated
		nbset nb(m.neighbors(i));	// add all direct neighbors to the list
//		const nbset &nb = m.neighbors(i);

		/** \note 	Add neighbors of neighboring points, some kind of coarse
		 * 			graining. the depth tells how many iterations from the
		 * 			central point will be made.
		 */
		size_t nb_depth = 0;
		while (nb_depth < depth) {
			for (nbset::iterator it = nb.begin(); it != nb.end(); ++it){
				const nbset &tmp = m.neighbors(*it);
				nb.insert(tmp.begin(), tmp.end());
			}
			nb_depth++;
		}

//!		Very simple approach : compute the mean value of cv
// 		compute the mean value of all the velocities computed using the neighbors
		if (method == "stupid") {
			Point v(0.,0.,0.);
			size_t err = 0;
			for (nbset::iterator it = nb.begin(); it != nb.end(); ++it){
				if ( act[*it] <= 0. ) 		{err++; continue;} 							// skip when a certain neighbor has no activation time
				if ( act[*it] == act[i]) 	{err++; continue;} 							// skip when velocity is zero

				v += (m.point(*it) - m.point(i))/ (act[*it] - act[i]) ;  			// units: um/ms, if input was um/ms
			}
			cv[i] = v / (double) (nb.size() - err) * 0.1;
													// convert from um/ms to cm/s
		} else if (method == "leastsquares") {
//!		More complex approach using linear least squares fit.

			vector<double> X(nb.size());
			vector<double> Y(nb.size());
			vector<double> Z(nb.size());
			vector<double> T(nb.size());

			double covx11, sumsqx;
			double covy11, sumsqy;
			double covz11, sumsqz;
			Point c(0.,0.,0.);
			size_t sz = 0;
			for (nbset::iterator it = nb.begin(); it != nb.end(); ++it){
				if ( act[*it] <= 0. ) 	 continue; 		// skip when a certain neighbor has no activation time
				X[sz] = m.point(*it).x - m.point(i).x;
				Y[sz] = m.point(*it).y - m.point(i).y;
				Z[sz] = m.point(*it).z - m.point(i).z;
				T[sz] = act[*it] - act[i];
				sz++;
			}

	//		 we solve the equation T c1 = X with T : delta activation time, X: delta x. c gives the conduction velocity.
			gsl_fit_mul(&T[0],1,&X[0],1, sz, &c.x, &covx11, &sumsqx);
			gsl_fit_mul(&T[0],1,&Y[0],1, sz, &c.y, &covy11, &sumsqy);
			gsl_fit_mul(&T[0],1,&Z[0],1, sz, &c.z, &covz11, &sumsqz);

//			cout << covx11 << " " << covy11 << " " << covz11 << " " << endl;

			try{
				cv[i] = c * .1; // convert from um/ms to cm/s
			} catch (Point::Exception_NaN &e){
				cerr << "Computation of conduction velocity failed for" << endl
					 << "tet " << i << " of " << m.points() << endl;

				cerr << e.what() << endl;
				cerr << " cv = " << c << endl
					 << "nb.size() = " << nb.size() << endl;
//				throw;
			}
	//			The standard deviations of the best-fit parameters are given
	//			by the square root of the corresponding diagonal elements of the
	//			covariance matrix, \sigma_{c_a} = \sqrt{C_{aa}}.

				stddev[i].x = sqrt(covx11/nb.size()); // mean squared error
				stddev[i].y = sqrt(covy11/nb.size());
				stddev[i].z = sqrt(covz11/nb.size());

		}

	}

	if (compute_mean){
		if (verbose) cmdline::msg(" - Computing mean value");
		vector<Point> tmp(m.points(), Point()); // conduction velocities
		for (size_t i = 0; i < m.points(); ++i){
			Point mean = cv[i];
			const nbset &nb = m.neighbors(i);
			for (nbset::iterator it = nb.begin(); it != nb.end(); ++it)
				mean += cv[*it];
			tmp[i] = mean / (nb.size() +1);
		}

		for (size_t i = 0; i < m.points(); ++i) cv[i] = tmp[i];
	}
///////////////////////////////// Write output /////////////////////////////////
	if (verbose) cmdline::msg(" - Writing output");
	/** Compute magnitude and its error:
	 *
	 * v = Sqrt[cx^2+cy^2+cz^2]
	 * dv = dv/dcx * Dcx + dv/dcy * Dcy + dv/dcz * Dcz
	 *    = (cx * dcx + cy * dcy + cz * dcz)/Sqrt[cx^2 + cy^2 + cz^2]
	 * 	  = c . dc / v
	 */

	double magnitude[m.points()];
	double magni_err[m.points()];
	#pragma omp parallel for
	for (size_t ii=0; ii<m.points(); ii++) {
		magnitude[ii] = cv[ii].abs();
		magni_err[ii] = cv[ii].dot(stddev[ii])/ magnitude[ii];
	}


	// check file extension
	if (not output.endswith(".dat") and not output.endswith(".vec") and not output.endswith(".igb.gz"))
		output.file_new_ext(".dat");										// igb.gz by default

	// save everything to a file
	if (output.endswith(".dat")){
		ofstream outfile(output.c_str());
		assert(!!outfile &&  "Error opening cv.dat");
		for (size_t ii=0; ii<m.points(); ii++)
			outfile << cv[ii].x << "\t" << cv[ii].y << "\t"  << cv[ii].z << endl;
		outfile.close();

		ofstream magfile( (output(0,-4) +"_mag.dat").c_str() );
		assert(!!magfile &&  "Error opening cv_mag.dat");
		for (size_t ii=0; ii<m.points(); ii++)	magfile << magnitude[ii] << endl;
		magfile.close();

		ofstream errfile( (output(0,-4) +"_mag_err.dat").c_str() );
		assert(!!magfile &&  "Error opening cv_mag_err.dat");
		for (size_t ii=0; ii<m.points(); ii++)	errfile << magni_err[ii] << endl;
		errfile.close();
	} else
	if (output.endswith(".vec")){ // output vector files
		ofstream outfile(output.c_str());
		assert(!!outfile &&  "Error opening cv.vec");
		outfile << m.points() << endl;
		for (size_t ii=0; ii<m.points(); ii++){
			outfile << m.p[ii].x << "\t" << m.p[ii].y  << "\t" << m.p[ii].z  << "\t"
					<<  cv[ii].x << "\t" <<  cv[ii].y << "\t"  <<  cv[ii].z << endl;
		}
		outfile.close();

		ofstream magfile( (output(0,-4) +"_mag.dat").c_str() );
		assert(!!magfile &&  "Error opening cv_mag.dat");
		for (size_t ii=0; ii<m.points(); ii++)	magfile << magnitude[ii] << endl;
		magfile.close();

		ofstream errfile( (output(0,-4) +"_mag_err.dat").c_str() );
		assert(!!magfile &&  "Error opening cv_mag_err.dat");
		for (size_t ii=0; ii<m.points(); ii++)	errfile << magni_err[ii] << endl;
		errfile.close();
	} else {
		{// save vector
			IGBheader igb;
			gzFile ou = gzopen(output.c_str(),"w");
			igb.fileptr(ou);
			igb.type(IGB_STRUCTURE);
			igb.taille(3*sizeof(double));					// size of the structure
			igb.struct_desc("3xdouble");
			igb.dim(m.points());
			igb.unites("cm/s");
			igb.write();

			size_t len = 0;
			for (size_t ii = 0; ii < m.points() ; ++ii){
				len += gzwrite(ou, &cv[ii].x, sizeof(double));
				len += gzwrite(ou, &cv[ii].y, sizeof(double));
				len += gzwrite(ou, &cv[ii].z, sizeof(double));
			}
			assert(len == m.points() * 3*sizeof(double) && "Error: Data written != 3");
			gzclose(ou);
		}

		{// save magnitude
			IGBheader igb;
			gzFile ou = gzopen((output(0,-7) +"_mag.igb.gz").c_str(),"w");
			igb.fileptr(ou);
			igb.type(IGB_DOUBLE);
			igb.dim(m.points());
			igb.unites("cm/s");
			igb.write();

			size_t len = gzwrite(ou, &magnitude[0], sizeof(double) * m.points());
			gzclose(ou);
			assert (len == sizeof(double) * m.points() && "Error : could not write cv_mag.igb.gz properly");
		}

		{// save error of magnitude
			IGBheader igb;
			gzFile ou = gzopen((output(0,-7) +"_mag_err.igb.gz").c_str(),"w");
			igb.fileptr(ou);
			igb.type(IGB_DOUBLE);
			igb.dim(m.points());
			igb.unites("cm/s");
			igb.write();

			size_t len = gzwrite(ou, &magni_err[0], sizeof(double) * m.points());
			gzclose(ou);
			assert (len == sizeof(double) * m.points() && "Error : could not write cv_mag_err.igb.gz properly");
		}
	}


//	for (size_t ii=0; ii<m.points(); ii++)
//			cout << stddev[ii].x << "\t" << stddev[ii].y << "\t"  << stddev[ii].z << endl;
	return 0;
}
